#include "EvolveAlgo.h"

#include "TotalTorque.h"
#include "OneLayerRungeKutta.h"
#include "TwoLayerRungeKutta.h"

#include <cmath>
#include <iostream>

EvolveAlgo::EvolveAlgo()
{

}

EvolveAlgo::~EvolveAlgo()
{

}

MultiLayer EvolveAlgo::solve(Clock clock,Pulse *field,Pulse *current,MultiLayer multiLayer,NewParameters params)
{
  Torque torque,torque1,torque2;
  int layerNumber=multiLayer.getSolveLayer();
  double *result;
  Vector3 tempMag;

  if(multiLayer.getNbFreeLayer()==1)
    {
      if(layerNumber==0)
	{
	  torque=TotalTorque(clock,field,current,multiLayer,layerNumber,params);   
	  tempMag=multiLayer[layerNumber].getMag();
	  result=rungekutta(torque,multiLayer[1],tempMag,clock);
	}
      else
	{
	  torque=TotalTorque(clock,field,current,multiLayer,layerNumber,params);   
	  tempMag=multiLayer[layerNumber].getMag();
	  result=rungekutta(torque,multiLayer[0],tempMag,clock);
	}
	  multiLayer[layerNumber].setMag(result[0],result[1],result[2]);
    }

  if(multiLayer.getNbFreeLayer()==2)
    {

      if(multiLayer.getNbLayer()==2)
	{
	  Layer pol;   //no pinned layer
	  pol.setMag(0,0,0);
	  torque1=TotalTorque(clock,field,current,multiLayer,0,params);
	  torque2=TotalTorque(clock,field,current,multiLayer,1,params);
	  if(layerNumber==0)
	    result=rungekutta3(multiLayer[0],multiLayer[1],pol,torque1,torque2,clock,current,params);
	  else if(layerNumber==1)
	    result=rungekutta3(multiLayer[1],multiLayer[0],pol,torque2,torque1,clock,current,params);
	}    
      if(multiLayer.getNbLayer()==3)
	{
	  torque1=TotalTorque(clock,field,current,multiLayer,layerNumber,params);
	  for(int i=0;i<3;i++)
	    {
	      if(multiLayer[i].getFrozen()==false && i!=layerNumber)
		{
		  torque2=TotalTorque(clock,field,current,multiLayer,i,params);
		  result=rungekutta3(multiLayer[layerNumber],multiLayer[i],multiLayer[3-layerNumber-i],torque1,torque2,clock,current,params);
		}
	    }
	}
    }
    
  Vector3 newMag;
  double norm;
  multiLayer[layerNumber].setMag(result[0],result[1],result[2]);
  if(multiLayer.getNbFreeLayer()==2)
    {
      for(int i=0;i<3;i++)
	{
	  if(multiLayer[i].getFrozen()==false&&layerNumber!=i)
	    {
	      multiLayer[i].setMag(result[3],result[4],result[5]);
	      norm=sqrt(multiLayer[i].getMag().dotProduct(multiLayer[i].getMag()));
	      newMag=multiLayer[i].getMag();
	      newMag.mult(1/norm);
	      multiLayer[i].setMag(newMag);
	    }
	}
    }
  delete [] result;
  norm=sqrt(multiLayer[layerNumber].getMag().dotProduct(multiLayer[layerNumber].getMag()));
  newMag=multiLayer[layerNumber].getMag();
  newMag.mult(1/norm);
  multiLayer[layerNumber].setMag(newMag);

  return multiLayer;
}
